clear all; clc; close all; clear memory;

load FERET_40x40
addpath 'utility'

fea = double(fea);
for train_num = 1:4
    %% Separate samples
    %------------------------------------------------
    % Compute the number of each class
    class_num = length(unique(gnd)); % Number of classes
    numClass = zeros(class_num,1);
    for i=1:class_num
        numClass(i,1) = length(find(gnd==i));
    end
    %------------------------------------------------
    % Seperate train and test
    trfea = []; ttfea = []; 
    trgnd = []; ttgnd = [];
    for j = 1:class_num
        index = find(gnd == j); 
        randIndex = 1:numClass(j);
        trfea = [trfea fea(:,index(randIndex(1:train_num)))];
        trgnd = [trgnd ; gnd(index(randIndex(1:train_num)))];
        ttfea = [ttfea fea(:,index(randIndex(train_num+1:end)))];
        ttgnd = [ttgnd ; gnd(index(randIndex(train_num+1:end)))];
    end
    % Nomalize all the samples
    trfea = trfea./repmat(sqrt(sum(trfea.*trfea)),[size(trfea,1) 1]);
    ttfea = ttfea./repmat(sqrt(sum(ttfea.*ttfea)),[size(ttfea,1) 1]);
    %------------------------------------------------
    
    trfea_hat=zeros(row*col,length(trgnd));
    for i=1:length(trgnd)
    % Generate naive Left and Right samples  
        tmpVec=trfea(:,i);
        tmpImg=reshape(tmpVec,row,col);               
        trLface(1:row,1:col/2)= tmpImg(1:row,1:col/2);
        trRface(1:row,col/2+1:col)= tmpImg(1:row,col/2+1:col);
        for km=1:col/2
            virRface(:,km)=trRface(:,col-(km-1));            
        end
        tr_z1=reshape(trLface(:,1:col/2),row*col/2,1);
        tr_z2=reshape(virRface(:,1:col/2),row*col/2,1);
        
    %------------------------------------------------
    % Generate approximate symmetrical face images from naive virtual training samples
        max_iter = 30;
        for kmn = 1:max_iter
            grad1 = tr_z1-tr_z2;
            tr_z1 = tr_z1-0.3/kmn*grad1;
            grad2 = tr_z2-tr_z1;
            tr_z2 = tr_z2-0.3/kmn*grad2;
            if norm(tr_z1-tr_z2) < 1.0e-2
                break;
            end 
        end

        virRface = reshape(tr_z2,row,col/2);
        optiR = zeros(size(virRface));
        for km=1:col/2            
            optiR(:,km) = virRface(:,col/2-km+1);
        end 
        optiRvec = reshape(optiR,row*col/2,1);
        virTr=[tr_z1;optiRvec];
   %------------------------------------------------
        trfea_hat(:,i)=virTr(:)/norm(virTr);
    end
    %% Virtual training samples have been generated by the above codes
    cclass = [];
    for i = 1:length(ttgnd)
        testVec=ttfea(:,i);
%         fprintf('The %dth testing sample is processing...\r\n',i); 

        x0 = trfea_hat'*testVec;
        lambda = 1e-2;
        [s, iterationCount] = Homotopy(trfea_hat, testVec, ...
                            'maxIteration', 5000,...
                            'stoppingCriterion', -1, ...
                            'groundtruth', x0, ...
                            'lambda', lambda, ...
                            'tolerance', 0.0001);
       
        % classification term
        gap = zeros(1,class_num);
        for j = 1:class_num
            temp_s           =  zeros(size(s));
            temp_s(j==trgnd) = s(j==trgnd);
            zz               =  ttfea(:,i)-trfea_hat*temp_s;
            gap(j)           =  zz(:)'*zz(:);      
        end
        index = find(gap==min(gap));
        cclass = [cclass; index(1)];
    end
    %% obtain the classification acc
    acc = 100*(sum(cclass==ttgnd))/size(ttfea,2);
    fprintf('Num_train is %d, Acc is %.2f%% \n',train_num,acc);
    clearvars -except train_num fea gnd row col
end

